//
//  testDevice.cpp
//  QuizMaster
//
//  Created by Jeppe Nielsen on 3/30/15.
//  Copyright (c) 2015 Jeppe Nielsen. All rights reserved.
//

#include <CoreFoundation/CoreFoundation.h>
#include <IOKit/IOKitLib.h>
#include <IOKit/IOCFPlugIn.h>
#include <IOKit/usb/IOUSBLib.h>
#include <IOKit/usb/USBSpec.h>

int main_testUSB() {

    CFMutableDictionaryRef matchingDictionary = NULL;
SInt32 idVendor = 0x045e; // set vendor id
SInt32 idProduct = 0x02a0; // set product id

//SInt32 idVendor = 0x062a; // set vendor id
//SInt32 idProduct = 0x4101; // set product id

io_iterator_t iterator = 0;
io_service_t usbRef;
SInt32 score;
IOCFPlugInInterface** plugin;
IOUSBDeviceInterface300** usbDevice = NULL;
IOReturn ret;
IOUSBConfigurationDescriptorPtr config;
IOUSBFindInterfaceRequest interfaceRequest;
IOUSBInterfaceInterface300** usbInterface;
char out[] = { 0x00, 0x00 }; // set data to send
char* in;
UInt32 numBytes;

matchingDictionary = IOServiceMatching(kIOUSBDeviceClassName);
CFDictionaryAddValue(matchingDictionary,
                     CFSTR(kUSBVendorID),
                     CFNumberCreate(kCFAllocatorDefault,
                                    kCFNumberSInt32Type, &idVendor));
CFDictionaryAddValue(matchingDictionary,
                     CFSTR(kUSBProductID),
                     CFNumberCreate(kCFAllocatorDefault,
                                    kCFNumberSInt32Type, &idProduct));
IOServiceGetMatchingServices(kIOMasterPortDefault,
                             matchingDictionary, &iterator);
usbRef = IOIteratorNext(iterator);
if (usbRef == 0)
{
    printf("Device not found\n");
    return -1;
}
IOObjectRelease(iterator);
IOCreatePlugInInterfaceForService(usbRef, kIOUSBDeviceUserClientTypeID,
                                  kIOCFPlugInInterfaceID, &plugin, &score);
IOObjectRelease(usbRef);
(*plugin)->QueryInterface(plugin,
                          CFUUIDGetUUIDBytes(kIOUSBDeviceInterfaceID300),
                          (LPVOID*)&usbDevice);
(*plugin)->Release(plugin);


ret = (*usbDevice)->USBDeviceOpen(usbDevice);
if (ret == kIOReturnSuccess)
{
    // set first configuration as active
    ret = (*usbDevice)->GetConfigurationDescriptorPtr(usbDevice, 0, &config);
    if (ret != kIOReturnSuccess)
    {
        printf("Could not set active configuration (error: %x)\n", ret);
        return -1;
    }
    (*usbDevice)->SetConfiguration(usbDevice, config->bConfigurationValue);
    
    printf("config numberInterfaces= %x \n", config->bNumInterfaces);
}
else if (ret == kIOReturnExclusiveAccess)
{
    // this is not a problem as we can still do some things
}
else
{
    printf("Could not open device (error: %x)\n", ret);
    return -1;
}

interfaceRequest.bInterfaceClass = kIOUSBFindInterfaceDontCare;
interfaceRequest.bInterfaceSubClass = kIOUSBFindInterfaceDontCare;
interfaceRequest.bInterfaceProtocol = kIOUSBFindInterfaceDontCare;
interfaceRequest.bAlternateSetting = kIOUSBFindInterfaceDontCare;
(*usbDevice)->CreateInterfaceIterator(usbDevice,
                                      &interfaceRequest, &iterator);
//IOIteratorNext(iterator); // skip interface #0
usbRef = IOIteratorNext(iterator);
IOObjectRelease(iterator);
IOCreatePlugInInterfaceForService(usbRef,
                                  kIOUSBInterfaceUserClientTypeID,
                                  kIOCFPlugInInterfaceID, &plugin, &score);
IOObjectRelease(usbRef);
(*plugin)->QueryInterface(plugin,
                          CFUUIDGetUUIDBytes(kIOUSBInterfaceInterfaceID300),
                          (LPVOID*)&usbInterface);
(*plugin)->Release(plugin);

ret = (*usbInterface)->USBInterfaceOpen(usbInterface);
if (ret != kIOReturnSuccess)
{
    printf("Could not open interface (error: %x)\n", ret);
    return -1;
}

// Send data through pipe 1
//(*usbInterface)->WritePipe(usbInterface, 1, out, sizeof(out));
 
// Read data through pipe 2

IOUSBInterfaceInterface300* inter = *usbInterface;

numBytes = 5;
in = (char*)malloc(numBytes);

int frame = 0;



while (true) {
    
    numBytes = 5;
    memset((void*)in, 0, numBytes);
    ret = inter->ReadPipe(usbInterface, 1, in, &numBytes);
    
    if (ret == kIOReturnSuccess)
    {
        char whichPlayer = in[2];
        printf("player = %d\n", whichPlayer);

        printf("Back: %s \n", ((in[3] & 0x20) == 0x20) ? "down" : "up");
        printf("Start: %s \n", ((in[3] & 0x10) == 0x10) ? "down" : "up");
        printf("Right: %s \n", ((in[3] & 0x08) == 0x08) ? "down" : "up");
        printf("Left: %s \n", ((in[3] & 0x04) == 0x04) ? "down" : "up");
        printf("Down: %s \n", ((in[3] & 0x02) == 0x02) ? "down" : "up");
        printf("Up: %s \n", ((in[3] & 0x01) == 0x01) ? "down" : "up");
        
        printf("Y: %s \n", ((in[4] & 0x80) == 0x80) ? "down" : "up");
        printf("X: %s \n", ((in[4] & 0x40) == 0x40) ? "down" : "up");
        printf("B: %s \n", ((in[4] & 0x20) == 0x20) ? "down" : "up");
        printf("A: %s \n", ((in[4] & 0x10) == 0x10) ? "down" : "up");
        printf("BigButton: %s \n", ((in[4] & 0x08) == 0x08) ? "down" : "up");
        printf("Xbox: %s \n", ((in[4] & 0x04) == 0x04) ? "down" : "up");
        
        //printf("Read %d bytes\n", numBytes);
    }
    else
    {
        printf("Read failed (error: %x)\n", ret);
    }
    
    usleep(160000);
    printf("-------- %d\n", frame++);
}

(*usbInterface)->USBInterfaceClose(usbInterface);
(*usbDevice)->USBDeviceClose(usbDevice);
 
return 0;
}